/* @author Stone at GDOU
   @date May 2021
   @brief A PID utility
 */
#include "pid.h"
 
 
PID::PID(float kp=0, float ki=0, float kd=0, float target=0) {
    this->kp = kp;
    this->ki = ki;
    this->kd = kd;
    this->target = target;
    this->last_err = 0;
    this->integ_err = 0;
    this->out = 0;
}


// do the calculations
void PID::Update(float x) {
    float _err = this->target - x;
    float _d_err = _err - this->last_err;
    this->integ_err += _err;
    
    // pid formula
    this->p_item = kp * _err;
    this->i_item = ki * integ_err;
    this->d_item = kd * _d_err;
    
    if (this->i_item > MAX_I_ITEM) {
        this->i_item = MAX_I_ITEM;
    }
    
    this->out =  p_item + i_item + d_item;
    
    // refresh
    this->last_err = _err;
}


// Clear the integral item
void PID::ClearHistory() {
    this->integ_err = 0;
}


